#ifndef BTL_CAMERA_CAMERAMODELTORDOFF
#define BTL_CAMERA_CAMERAMODELTORDOFF

#include <btl/Camera/CameraModel.hpp>

namespace btl {
namespace camera {

class CameraModelTordoff : public CameraModelPinhole, public CameraModelDistortedBidirectional
{

   public:

      CameraModelTordoff(
         int imageResolutionX, int imageResolutionY,
         double focalLength,
         double principlePointX, double principlePointY,
         double radialDistortionCoeff);

      CameraModelTordoff(
         const Eigen::Vector2i& imageResolution,
         double focalLength,
         const Eigen::Vector2d& principlePoint,
         double radialDistortionCoeff);

      CameraModelTordoff(
         int imageResolutionX, int imageResolutionY,
         double focalLengthX, double focalLengthY,
         double principlePointX, double principlePointY,
         double radialDistortionCoeff);

      CameraModelTordoff(
         const Eigen::Vector2i& imageResolution,
         const Eigen::Vector2d& focalLength,
         const Eigen::Vector2d& principlePoint,
         double radialDisortionCoeff);

      ~CameraModelTordoff();

      Eigen::Vector2d distortPoint(const Eigen::Vector2d& undistorted);

      Eigen::Vector2d undistortPoint(const Eigen::Vector2d& distorted);

   protected:

      double _radialDistortionCoeff;

};

} //namespace camera
} //namespace btl

namespace btl {

   using camera::CameraModelTordoff;

} //namespace btl

namespace btl {
namespace camera {

inline CameraModelTordoff::CameraModelTordoff(
   int imageResolutionX, int imageResolutionY,
   double focalLength,
   double principlePointX, double principlePointY,
   double radialDistortionCoeff
):
   CameraModelPinhole(imageResolutionX, imageResolutionY, focalLength, principlePointX, principlePointY),
   _radialDistortionCoeff(radialDistortionCoeff)
{}

inline CameraModelTordoff::CameraModelTordoff(
   const Eigen::Vector2i& imageResolution,
   double focalLength,
   const Eigen::Vector2d& principlePoint,
   double radialDistortionCoeff
) :
   CameraModelPinhole(imageResolution, focalLength, principlePoint),
   _radialDistortionCoeff(radialDistortionCoeff)
{}

inline CameraModelTordoff::CameraModelTordoff(
   int imageResolutionX, int imageResolutionY,
   double focalLengthX, double focalLengthY,
   double principlePointX, double principlePointY,
   double radialDistortionCoeff
) :
   CameraModelPinhole(imageResolutionX, imageResolutionY, focalLengthX, focalLengthY, principlePointX, principlePointY),
   _radialDistortionCoeff(radialDistortionCoeff)
{}

inline CameraModelTordoff::CameraModelTordoff(
   const Eigen::Vector2i& imageResolution,
   const Eigen::Vector2d& focalLength,
   const Eigen::Vector2d& principlePoint,
   double radialDistortionCoeff
) :
   CameraModelPinhole(imageResolution, focalLength, principlePoint),
   _radialDistortionCoeff(radialDistortionCoeff)
{}

inline CameraModelTordoff::~CameraModelTordoff() {}

inline Eigen::Vector2d CameraModelTordoff::distortPoint(const Eigen::Vector2d& undistorted)
{
   Eigen::Vector2d vec = undistorted - _principlePoint;
   double invFactor = 1.0 / sqrt(1.0 - 2.0 * _radialDistortionCoeff * vec.squaredNorm());
   Eigen::Vector2d distorted = _principlePoint + (vec * invFactor);
   return distorted;
}

inline Eigen::Vector2d CameraModelTordoff::undistortPoint(const Eigen::Vector2d& distorted)
{
   Eigen::Vector2d vec = distorted - _principlePoint;
   double invFactor = 1.0 / sqrt(1.0 + 2.0 * _radialDistortionCoeff * vec.squaredNorm());
   Eigen::Vector2d undistorted = _principlePoint + (vec * invFactor);
}

} //namespace camera
} //namespace btl

#endif //BTL_CAMERA_CAMERAMODELTORDOFF
